#include <stdio.h>

#include "stm32f10x.h"
#include "stm32f10x_conf.h"
#include "serial.h"
#include "bcd100.h"
#include "delay.h"
#include "encoder.h"
#include "commands.h"
#include "sound.h"
#include "stepm.h"

//#define SWD_OFF

#define BCD100_RESET_PORT  GPIOC
#define BCD100_RESET_PIN   GPIO_Pin_13

#define ENCODERS_POWER_PORT	GPIOA
#define ENCODERS_POWER_PIN	GPIO_Pin_4

#define OBSTACLES_SENSOR_PORT		GPIOA
#define OBSTACLES_FORWARD_PIN		GPIO_Pin_5
#define OBSTACLES_BACKWARD_PIN	GPIO_Pin_6

void RCC_Configuration(void);
void GPIO_Configuration(void);
void PWM_Configuration(TIM_TypeDef* TIMx);
void PWM_Configuration_36K(TIM_TypeDef* TIMx);
void ADC_Configuration(void);

int main(void)
{      
  int Bluetooth_present = 0;
  char cmd_buf[10];
  unsigned short *sound_buffer = 0;
  
  /* System timer configuration: ticks every 1ms */
  SysTick_Config(SystemCoreClock / 1000);
  
  /* Scheduler initialization */
  Scheduler_Initialize();
  
  /* System Clocks Configuration */
  RCC_Configuration();

  /* GPIO Configuration */
  GPIO_Configuration();

  /* PWM Configuration for motor speed control */
  //PWM_Configuration(TIM3);
  /* PWM Configuration for IR obstacles sensors */
  //PWM_Configuration_36K(TIM15);
  
  /* Encoders configuration */
  //ENC_Configuration(TIM1);
  //ENC_Configuration(TIM2);
  /* Update encoders every ENC_UPDATE_MS */
  //AddScheduledTask(updateEncoders, ENC_UPDATE_MS);
  
  /* ADC configuration */
  //ADC_Configuration();
  //Sound_Initialize();
  
  /* Initialize serial port on speed 9600 with both timeouts 50ms */
  Serial_Initialize(9600, 50, 50);
  
  /* Prepare bluetooth module */
  //Bluetooth_present = BCD100_Configuration(BCD100_RESET_PORT, BCD100_RESET_PIN);
  //if(!Bluetooth_present)
  //  printf(ERROR "No bluetooth module found\r\n");
  
  /* Enable motors 1 and 2 */
  //EnableMotor(1);
  //EnableMotor(2);
  
  //SetSpeed(2,1000);
  /* Clear receive buffer of serial before we get something */
  Serial_Clear();
  
  
  StepMotorSetup();
  
  while (1)
  {   
    /* Get and process sound frame from microphone */
    sound_buffer = Sound_PollBuffer();
    if(sound_buffer)
    {
	sound_buffer = 0;
    }
    
    /* Get and process command from host */
    gets_trim(cmd_buf);  
    if(cmd_buf[0])
      CMD_Process(cmd_buf);
    
    /* Obstacles sensors processing */
    /*if(!GPIO_ReadInputDataBit(OBSTACLES_SENSOR_PORT, OBSTACLES_FORWARD_PIN))
    {
	if(GetSpeed(1) > 0 || GetSpeed(2) > 0)
	{
	  StopMotor(1);
	  StopMotor(2);
	  printf(WARNING "Obstacle in front of tank\n");
	}
    }
    if(!GPIO_ReadInputDataBit(OBSTACLES_SENSOR_PORT, OBSTACLES_BACKWARD_PIN))
    {
	if(GetSpeed(1) < 0 || GetSpeed(2) < 0)
	{
	  StopMotor(1);
	  StopMotor(2);
	  printf(WARNING "Obstacle is behind of tank\n");
	}
    }*/
  }
}

void RCC_Configuration(void)
{
  /* TIM3 and TIM2 clock enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 |
				 RCC_APB1Periph_TIM2, ENABLE);

  /* TIM1, GPIOA, GPIOB, GPIOC, AFIO, TIM1, TIM15 and USART1 clock enable */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA |
                         RCC_APB2Periph_GPIOB |
                         RCC_APB2Periph_GPIOC |
                         RCC_APB2Periph_USART1 |
                         RCC_APB2Periph_TIM1 |
				 RCC_APB2Periph_TIM15 |
                         RCC_APB2Periph_AFIO, ENABLE);
}

void GPIO_Configuration(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
  
  GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
#ifdef SWD_OFF  
  GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable, ENABLE);
#endif

  GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE);	
 
  /* USART pins configuration */
  
  /* Configure USART1 Rx as input floating */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPIOB, &GPIO_InitStructure);
  
  /* Configure USART1 Tx as alternate function push-pull */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_Init(GPIOB, &GPIO_InitStructure);
  
  GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);	

  /* Configure the BCD100 reset pin */
  GPIO_InitStructure.GPIO_Pin = BCD100_RESET_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(BCD100_RESET_PORT, &GPIO_InitStructure);
  
  /* Configure TIM1 encoder interface */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  /* Configure TIM2 encoder interface */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  
  /* Configure power pin for encoder's leds  */
  GPIO_InitStructure.GPIO_Pin = ENCODERS_POWER_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(ENCODERS_POWER_PORT, &GPIO_InitStructure);
  GPIO_SetBits(ENCODERS_POWER_PORT, ENCODERS_POWER_PIN);
  
  /* Configure IR led PWMs */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  
  /* Configure forward obstacles sensor's pin  */
  GPIO_InitStructure.GPIO_Pin = OBSTACLES_FORWARD_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
  GPIO_Init(OBSTACLES_SENSOR_PORT, &GPIO_InitStructure);
  /* Configure backward obstacles sensor's pin  */
  GPIO_InitStructure.GPIO_Pin = OBSTACLES_BACKWARD_PIN;
  GPIO_Init(OBSTACLES_SENSOR_PORT, &GPIO_InitStructure);
}

void PWM_Configuration(TIM_TypeDef* TIMx)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_OCInitTypeDef  TIM_OCInitStructure;   

  /* -----------------------------------------------------------------------
    TIMx Configuration: generate 4 PWM signals with 4 different duty cycles:
    The TIMxCLK frequency is set to SystemCoreClock (Hz), to get TIMx counter
    clock at 24 MHz the Prescaler is computed as following:
     - Prescaler = (TIMxCLK / TIMx counter clock) - 1
    SystemCoreClock is set to 72 MHz for Low-density, Medium-density, High-density
    and Connectivity line devices and to 24 MHz for Low-Density Value line and
    Medium-Density Value line devices

    The TIMx is running at 24 KHz: TIMx Frequency = TIMx counter clock/(ARR + 1)
                                                  = 24 MHz / 1000 = 24 KHz
    TIMx Channel1 duty cycle = (TIMx_CCR1/ TIM3_ARR)* 100
    TIMx Channel2 duty cycle = (TIMx_CCR2/ TIM3_ARR)* 100
    TIMx Channel3 duty cycle = (TIMx_CCR3/ TIM3_ARR)* 100
    TIMx Channel4 duty cycle = (TIMx_CCR4/ TIM3_ARR)* 100
  ----------------------------------------------------------------------- */
  /* Compute the prescaler value */
  uint16_t PrescalerValue = (uint16_t) (SystemCoreClock / 10000000) - 1;//10//10//10
  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = 999;
  TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

  TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);

  /* PWM1 Mode configuration: Channel1 to Channel4 */
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  
  TIM_OCInitStructure.TIM_Pulse = 0;
  TIM_OC1Init(TIMx, &TIM_OCInitStructure);
  TIM_OC1PreloadConfig(TIMx, TIM_OCPreload_Enable);
  TIM_OC2Init(TIMx, &TIM_OCInitStructure);
  TIM_OC2PreloadConfig(TIMx, TIM_OCPreload_Enable);
  TIM_OC3Init(TIMx, &TIM_OCInitStructure);
  TIM_OC3PreloadConfig(TIMx, TIM_OCPreload_Enable);
  TIM_OC4Init(TIMx, &TIM_OCInitStructure);
  TIM_OC4PreloadConfig(TIMx, TIM_OCPreload_Enable);

  TIM_ARRPreloadConfig(TIMx, ENABLE);

  /* TIMx enable counter */
  TIM_Cmd(TIMx, ENABLE);
}

void PWM_Configuration_36K(TIM_TypeDef* TIMx)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_OCInitTypeDef  TIM_OCInitStructure;   

  /* -----------------------------------------------------------------------
    TIMx Configuration: generate 4 PWM signals with 4 different duty cycles:
    The TIMxCLK frequency is set to SystemCoreClock (Hz), to get TIMx counter
    clock at 24 MHz the Prescaler is computed as following:
     - Prescaler = (TIMxCLK / TIMx counter clock) - 1
    SystemCoreClock is set to 72 MHz for Low-density, Medium-density, High-density
    and Connectivity line devices and to 24 MHz for Low-Density Value line and
    Medium-Density Value line devices

    The TIMx is running at 36 KHz: TIMx Frequency = TIMx counter clock/(ARR + 1)
                                                  = 24 MHz / 667 = 36 KHz
    TIMx Channel1 duty cycle = 50
    TIMx Channel2 duty cycle = 50
  ----------------------------------------------------------------------- */
  /* Compute the prescaler value */
  uint16_t PrescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1;
  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = 666;
  TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

  TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);

  /* PWM1 Mode configuration: Channel1 to Channel4 */
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  
  TIM_OCInitStructure.TIM_Pulse = 333;
  TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
  TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
  TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
  TIM_OC1Init(TIMx, &TIM_OCInitStructure);
  TIM_OC1PreloadConfig(TIMx, TIM_OCPreload_Enable);
  TIM_OC2Init(TIMx, &TIM_OCInitStructure);
  TIM_OC2PreloadConfig(TIMx, TIM_OCPreload_Enable);

  TIM_ARRPreloadConfig(TIMx, ENABLE);

  /* TIMx enable counter */
  TIM_Cmd(TIMx, ENABLE);
}

void ADC_Configuration(void)
{
  ADC_InitTypeDef  ADC_InitStructure;
  /* PCLK2 is the APB2 clock */
  /* ADCCLK = PCLK2/6 = 72/6 = 12MHz*/
  RCC_ADCCLKConfig(RCC_PCLK2_Div6);

  /* Enable ADC1 clock so that we can talk to it */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
  /* Put everything back to power-on defaults */
  ADC_DeInit(ADC1);

  /* ADC1 Configuration ------------------------------------------------------*/
  /* ADC1 and ADC2 operate independently */
  ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
  /* Disable the scan conversion so we do one at a time */
  ADC_InitStructure.ADC_ScanConvMode = DISABLE;
  /* Don't do contimuous conversions - do them on demand */
  ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
  /* Start conversin by software, not an external trigger */
  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  /* Conversions are 12 bit - put them in the lower 12 bits of the result */
  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
  /* Say how many channels would be used by the sequencer */
  ADC_InitStructure.ADC_NbrOfChannel = 1;

  /* Now do the setup */
  ADC_Init(ADC1, &ADC_InitStructure);
  /* Enable ADC1 */
  ADC_Cmd(ADC1, ENABLE);

  /* Enable ADC1 reset calibaration register */
  ADC_ResetCalibration(ADC1);
  /* Check the end of ADC1 reset calibration register */
  while(ADC_GetResetCalibrationStatus(ADC1));
  /* Start ADC1 calibaration */
  ADC_StartCalibration(ADC1);
  /* Check the end of ADC1 calibration */
  while(ADC_GetCalibrationStatus(ADC1));
}

#ifdef  USE_FULL_ASSERT

void assert_failed(uint8_t* file, uint32_t line)
{
  printf(ERROR "Wrong parameters: file %s on line %d\n", file, line);
  while(1);
}
#endif

